#include "digita.hpp"
#include "tost.hpp"
#include <thread>
#include <mutex>
#include <stdlib.h>
#include <stdio.h>
#include <iostream>
#include <math.h>
#include <unistd.h>
#include <fstream>
#include <sstream>
#include <string>
#include <cstdlib>
#include <iomanip>
#include <Dstar.h>
#include <Dstar.cpp>
#include <list>
#include "opencv2/ximgproc.hpp"
using namespace std;
int objID;
int cntmissed = 0;
int cntfalse = 0;
int previousID = 0;
int currentID = 0;
float targetX, targetY, targetZ, previousX, previousY, previousZ, firstID, firstX, firstY, firstZ,secondID, secondX,secondY,secondZ;
int initX,initY,curX,curY,endX,endY;
bool isHuman = false;
bool correctDirection = false;
vector<int>planningAngle;
int humanAngle;
int i = 1;
bool nofile = true;
int k0;
#include "platemap.hpp"
#include "doublebf2.hpp"
#include "usbcamera.hpp"


static void ff(const cv::Mat &bw, std::vector<cv::Point> pts[9])
{
    for(int y=1;y<bw.rows-1;y++)
    {
        for(int x=1;x<bw.cols-1;x++)
        {
            if(bw.at<uchar>(y,x)>0)
            {
                int count=0;

                if(bw.at<uchar>(y-1,x)>0) count++;
                if(bw.at<uchar>(y,x-1)>0) count++;
                if(bw.at<uchar>(y,x+1)>0) count++;
                if(bw.at<uchar>(y+1,x)>0) count++;


                if(bw.at<uchar>(y+1,x+1)>0 && bw.at<uchar>(y,x+1)==0 && bw.at<uchar>(y+1,x)==0) count++;
                if(bw.at<uchar>(y-1,x-1)>0 && bw.at<uchar>(y,x-1)==0 && bw.at<uchar>(y-1,x)==0) count++;
                if(bw.at<uchar>(y+1,x-1)>0 && bw.at<uchar>(y,x-1)==0 && bw.at<uchar>(y+1,x)==0) count++;
                if(bw.at<uchar>(y-1,x+1)>0 && bw.at<uchar>(y,x+1)==0 && bw.at<uchar>(y-1,x)==0) count++;


                pts[count].push_back(cv::Point(x,y));
            }
        }
    }
}





static void cap(DoubleBf2<cv::Mat> &bf, int &k)
{

//        cv::VideoCapture c("/home/u/TV_CAM_设备_20180427_111735.mkv_20180427_115839.avi");

    USBCamera c;
    c.open(1);


    cv::Mat K,D;
    USBCamera::Load(K, "Kf.yml");
    USBCamera::Load(D, "Df.yml");

    USBCamera::ScaleK(K, c.get(CV_CAP_PROP_FRAME_WIDTH)/1280, c.get(CV_CAP_PROP_FRAME_HEIGHT)/720);


    cv::Size sznew(1920,1080) ;
    cv::Mat Knew=K.clone();
    Knew.at<double>(0,2)=sznew.width/2;
    Knew.at<double>(1,2)=sznew.height/2;


    cv::Mat R;
    cv::Mat rv(3, 1, CV_64FC1, cv::Scalar(0));
    rv.at<double>(0)=0*CV_PI/180;
    cv::Rodrigues(rv, R);


    cv::Mat map1, map2;
    cv::fisheye::initUndistortRectifyMap(K, D, R, Knew, sznew, CV_16SC2, map1, map2);


    std::cout<<"map1="<<map1.size()<<"\n"<<std::flush;
    std::cout<<"map2="<<map2.size()<<"\n"<<std::flush;



    cv::Mat *pm=NULL;
    cv::Mat frame;
    while(k0!=27)
    {
        if(c.read(frame))
        {
            pm=new cv::Mat();
            cv::remap(frame, *pm, map1, map2, cv::INTER_LINEAR);
            //            cv::imwrite("pm.png",*pm);
            bf.Write(pm);
        }
        else
        {
            //            k=27;
            pm=NULL;
            bf.Write(pm);
            break;
        }
                std::this_thread::sleep_for(std::chrono::milliseconds(90));
//        std::this_thread::yield();
    }
    c.release();
}


static void proc(DoubleBf2<cv::Mat> &bf, int &k, DoubleBf2<AnalysisResultA> &bf2, double propertyThres=0.1)
{

    PlateBaseB pb;

    cv::Mat K;
    cv::Mat D;
    double angle=0;

    USBCamera::Load(K, "Kf.yml");
    USBCamera::ScaleK(K, 1920./1280, 1080./720);
    cv::Size sznew(1920,1080) ;
    K.at<double>(0,2)=sznew.width/2;
    K.at<double>(1,2)=sznew.height/2;


    cv::Size gap(30,30);
    cv::Rect oldnbb[2];
    double oldp=0;

    AnalysisResultA *par=NULL;

    while(k!=27)
    {
        cv::Mat *pm=NULL;

        bf.Read(pm);

        if(pm!=NULL)
        {
            par=new AnalysisResultA();

            if(oldp>propertyThres)
            {
                par->roiDigit[0]=oldnbb[0];
                par->roiDigit[1]=oldnbb[1];
                pb.TraceTwoDigit(*pm, K, D, angle, *par, gap);
            }
            else
                pb.LocateTwoDigit(*pm, K, D, angle, *par);

            oldp=par->property;
            if(oldp>propertyThres)
            {
                oldnbb[0]=par->roiDigit[0];
                oldnbb[1]=par->roiDigit[1];
                gap.width=gap.height=20+(-par->location[1]-6000)*(60-20)/(1000-6000);
            }

            //            std::cout<<"gap="<<gap<<"\n"<<std::flush;

            par->frame=pm;

            bf2.Write(par);

            //        delete pm;
        }
        else
        {
            //            k=27;
            break;
        }
    }

    par=NULL;
    bf2.Write(par);
}


static void proc2(DoubleBf2<cv::Mat> &bf, int &k, DoubleBf2<AnalysisResultA> &bf2)
{

    PlateMap pmap;

    cv::Mat K;
    cv::Mat D;
    double angle=0;

    USBCamera::Load(K, "Kf.yml");
    USBCamera::ScaleK(K, 1920./1280, 1080./720);
    cv::Size sznew(1920,1080) ;
    K.at<double>(0,2)=sznew.width/2;
    K.at<double>(1,2)=sznew.height/2;

    AnalysisResultA *par=NULL;

    while(k0!=27)
    {
        cv::Mat *pm=NULL;

        std::cout<<"read pm\n"<<std::flush;
        bf.Read(pm);

        if(pm!=NULL)
        {
            std::cout<<"analysis pm\n"<<std::flush;

            par=new AnalysisResultA();

           pmap.UpdateLocation(*pm, K, D, angle, *par);

            par->frame=pm;

            bf2.Write(par);

            //        delete pm;
        }
        else
        {
            std::cout<<"null pm\n"<<std::flush;
            //            k=27;
            break;
        }
    }

    par=NULL;
    bf2.Write(par);
}


static void c2()
{
    std::string path="/home/u/TV_CAM_设备_20180427_111735.mkv_20180427_115839.avi";

    cv::namedWindow(path,cv::WINDOW_NORMAL);
    cv::moveWindow(path,32,32);
    cv::resizeWindow(path, 1280, 720);

    DoubleBf2<cv::Mat> bf;
    DoubleBf2<AnalysisResultA> bf2;

    int k=0;

    std::thread th(cap, std::ref(bf), std::ref(k));

    std::thread th2(proc2, std::ref(bf), std::ref(k), std::ref(bf2));

    while(k0!=27)
    {
        AnalysisResultA *par=NULL;
        bf2.Read(par);
        if(par!=NULL)
        {
            par->Draw();
            cv::imshow(path, *(par->frame));
            k=cv::waitKey(10);
            delete par;
        }
        else
        {
            k0=27;
        }
    }

    if(th.joinable()) th.join();

    if(th2.joinable()) th2.join();


    cv::destroyWindow(path);

}


static void st()
{
    std::string path="/home/u/TV_CAM_设备_20180427_111735.mkv_20180427_115839.avi";

    cv::VideoCapture c(path);
    cv::Mat m;
    cv::namedWindow(path,cv::WINDOW_NORMAL);
    cv::moveWindow(path,32,32);
    cv::resizeWindow(path, 1280, 720);


    cv::Mat K,D;
    USBCamera::Load(K, "Kf.yml");
    USBCamera::Load(D, "Df.yml");

    USBCamera::ScaleK(K, c.get(CV_CAP_PROP_FRAME_WIDTH)/1280, c.get(CV_CAP_PROP_FRAME_HEIGHT)/720);


    cv::Size sznew(1920,1080) ;
    cv::Mat Knew=K.clone();
    Knew.at<double>(0,2)=sznew.width/2;
    Knew.at<double>(1,2)=sznew.height/2;


    cv::Mat R;
    cv::Mat rv(3, 1, CV_64FC1, cv::Scalar(0));
    rv.at<double>(0)=0*CV_PI/180;
    cv::Rodrigues(rv, R);


    cv::Mat map1, map2;
    cv::fisheye::initUndistortRectifyMap(K, D, R, Knew, sznew, CV_16SC2, map1, map2);


    std::cout<<"map1="<<map1.size()<<"\n"<<std::flush;
    std::cout<<"map2="<<map2.size()<<"\n"<<std::flush;


    PlateBaseA pb;



    cv::Size gap(30,30);
    cv::Rect nbb[2];

    cv::Mat bwDigit[2];

    cv::Mat bwDigitZS[2];

    cv::Mat bwDigitGH[2];

    double propertyThres=0.1;

    double resp=0;

    int i=0;

    while(c.read(m))
    {

        cv::Mat mm;
        {
            //        int64 t0=cv::getTickCount();
            cv::remap(m, mm, map1, map2, cv::INTER_LINEAR);
            //        int64 t1=cv::getTickCount();

            //        std::cout<<"\nt="<<(double)(t1-t0)/cv::getTickFrequency()<<"\n"<<std::flush;
        }

        if(i>200 && i<760)
        {


            int64 t0=cv::getTickCount();

            if(resp<propertyThres)
                resp=pb.LocateTwoDigit(mm, nbb, bwDigit);
            else
                resp=pb.TraceTwoDigit(mm, nbb, bwDigit, gap);

            int64 t1=cv::getTickCount();


            cv::imshow("6",bwDigit[0]);
            cv::imshow("7",bwDigit[1]);


            {
                cv::Mat tmp(bwDigit[0].size()+cv::Size(2,2), bwDigit[0].type(), cv::Scalar(0));
                bwDigit[0].copyTo(tmp(cv::Rect(cv::Point(1,1),bwDigit[0].size())));
                cv::ximgproc::thinning(tmp, bwDigitZS[0], cv::ximgproc::THINNING_ZHANGSUEN);
                cv::ximgproc::thinning(tmp, bwDigitGH[0], cv::ximgproc::THINNING_GUOHALL);
            }

            {
                cv::Mat tmp(bwDigit[1].size()+cv::Size(2,2), bwDigit[1].type(), cv::Scalar(0));
                bwDigit[1].copyTo(tmp(cv::Rect(cv::Point(1,1),bwDigit[1].size())));
                cv::ximgproc::thinning(tmp, bwDigitZS[1], cv::ximgproc::THINNING_ZHANGSUEN);
                cv::ximgproc::thinning(tmp, bwDigitGH[1], cv::ximgproc::THINNING_GUOHALL);
            }

            cv::imshow("6ZS",bwDigitZS[0]);
            cv::imshow("7ZS",bwDigitZS[1]);

            cv::imshow("6GH",bwDigitGH[0]);
            cv::imshow("7GH",bwDigitGH[1]);

            std::cout<<"\nt="<<(double)(t1-t0)/cv::getTickFrequency()<<",\t"<<resp<<"\n"<<std::flush;
            if(resp>propertyThres)
            {
                //                cv::Matx33d pm=pb.GetPlateProjection(mm, nbb);

                //                pb.ShowProj(mm, pm);

                cv::rectangle(mm, nbb[0], cv::Scalar(0,0,255), 5);
                cv::rectangle(mm, nbb[1], cv::Scalar(0,255,0), 5);
            }
        }


        std::cout<<"i="<<i++<<"\n"<<std::flush;

        cv::imshow(path, mm);
        cv::waitKey(1);
    }

    c.release();
}


int main()
{


    c2();
    return 0;

    //    USBCamera().testfps(1,"");


    //    PlateBaseA::ttest("67r.png","kroi.png");



    return 0;
}
